/*  Copyright Michael Otte, University of Colorado, 9-9-2009
 *
 *  This file is part of Base_Planner_CU.
 *
 *  Base_Planner_CU is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  Base_Planner_CU is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Base_Planner_CU. If not, see <http://www.gnu.org/licenses/>.
 *
 *
 *  If you require a different license, contact Michael Otte at
 *  michael.otte@colorado.edu
 */

// this node assumes map is received as a probability occupancy grid
// cost is determined by the function prob_to_cost().

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <sys/types.h>
#include <time.h>
#include <math.h>
#include <list>
#include <sstream>

#include <ros/ros.h>

#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Point32.h"

#include "nav_msgs/Path.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/GridCells.h"

#include "sensor_msgs/PointCloud.h"

#include "std_msgs/Int32.h"

#include "localization_cu/GetPose.h"

#define MAINFILE
#define NOGLUT
#define CPP
#include "fielddstaroptimized_gradient_extract.c"

#ifndef PI
  #define PI 3.1415926535897
#endif
        
struct MAP;
typedef struct MAP MAP;
        
struct POSE;
typedef struct POSE POSE;

struct UPDATE;
typedef struct UPDATE UPDATE;

// globals for transform, these can be reset via the parameter server, see main()
float OBSTACLE_COST = 10000;         // the cost of an obstacle (of probability 1) note: this should be greater than the max path length
float robot_radius = .2;             // (m), radius of the robot
float safety_distance = .1;          // (m),  distance that must be maintained between robot and obstacles
float old_path_discount = .95;       // if (current path length)*old_path_discount < (old path length) < (current path length)/old_path_discount, then stick with the old path
float MAX_POSE_JUMP = 25;            // (map grids) after which it is easer to replan from scratch
bool high_cost_safety_region = true; // true: dilate obstacles by an extra extra_dilation but instad of lethal, multiply existing cost by extra_dilation_mult
float extra_dilation = .2;           // (m)

// global ROS subscriber handles
ros::Subscriber pose_sub;
ros::Subscriber goal_sub;
ros::Subscriber map_changes_sub;

// global ROS publisher handles
ros::Publisher global_path_pub;
ros::Publisher system_update_pub;

// this stores the raw map as it is received from the mapping system
MAP* raw_map = NULL;
        
// globals for robot and goal
POSE* robot_pose = NULL;
POSE* goal_pose = NULL;


bool new_goal = false;       
bool change_token_used = false;
bool reload_map = false; 

float extra_dilation_mult = OBSTACLE_COST/2;
bool extra_dilation_mult_from_dist = true; // when true (and also using high cost safety region), this causes extra_dilation_mult to be calculated as a function of distance (i.e. so that the path is pushed to the center of narrow hallways)


/*---------------------- MAP --------------------------------------------*/
struct MAP
{
    float** cost;
    int height;
    int width;
    float resolution;
};

// this creates and returns a pointer to a map struct
MAP* make_map(int height, int width, float resolution)
{
  MAP* map = (MAP*)calloc(1, sizeof(MAP));
  map->height = height;
  map->width = width;
  map->resolution = resolution; 
  
  map->cost = (float**)calloc(height, sizeof(float*));
        
  for (int y = 0; y < height; y++)  
  {
	map->cost[y] = (float*)calloc(width, sizeof(float));  
  } 
  return map;
}


// this deallocates all required memory for a map
void destroy_map(MAP* map)
{
  int y;
  if(map != NULL)
  {
    for (y = 0; y < map->height; y++)  
    {
	  if(map->cost[y] != NULL)
        free(map->cost[y]);

    }

    if(map->cost != NULL)
      free(map->cost);

    map->height = 0;
    map->width = 0;
    free(map);
  }
}

// prints map on command line
void print_map(MAP* map)
{
  int y, x;

  if(map->cost != NULL)
  {
    printf("\n");  
    for (y = 0; y < map->height; y++)  
    {
      for (x = 0; x < map->width; x++)  
	    printf(" %f", map->cost[y][x]);
      printf("\n");  
    }
    printf("\n");  
  } 
}

/* ----------------------- POSE -----------------------------------------*/
struct POSE
{
    float x;
    float y;
    float z;
    
    float qw;
    float qx;
    float qy;
    float qz; 
    
    float alpha; // rotation in 2D plane
    
    float cos_alpha;
    float sin_alpha;
};


// this creates and returns a pointer to a POSE struct
POSE* make_pose(float x, float y, float z)
{
  POSE* pose = (POSE*)calloc(1, sizeof(POSE));
  pose->x = 0;
  pose->y = 0;
  pose->z = 0;
  pose->alpha = 0;

  float r = sqrt(2-(2*cos(pose->alpha)));

  if(r == 0)
    pose->qw = 1;
  else
    pose->qw = sin(pose->alpha)/r; 

  pose->qx = 0;
  pose->qy = 0;
  pose->qz = r/2; 

  pose->cos_alpha = cos(pose->alpha);
  pose->sin_alpha = sin(pose->alpha);
  
  return pose;
}


// this deallocates all required memory for a POSE
void destroy_pose(POSE* pose)
{
  if(pose != NULL)
    free(pose);
}

// prints pose on command line
void print_pose(POSE* pose)
{
  if(pose != NULL)
  {
    printf("\n");  
    printf("x: %f, y: %f, z: %f \n",pose->x, pose->y, pose->z);  
    printf("qw: %f, qx: %f, qy: %f, qz: %f \n", pose->qw, pose->qx, pose->qy, pose->qz);  
    printf("\n");  
  } 
}

// converts an obstacle probability [0 ... 1] into a cost >= 1
float prob_to_cost(float prob)
{
  if(prob < .3)
    return 1;
  else
    return OBSTACLE_COST;  
}
        
// this transfers the data from raw_map to the search map, dilating it 
// by robot_radius + safety_distance during the transfer. 
void populate_map_from_raw_map()
{
  if(raw_map == NULL)
  {
    printf(" trying to update from invalid raw map\n"); 
    return;
  }
  
  if(map == NULL)
  {
    printf(" trying to update to invalid map\n"); 
    return;
  }
  
  float dilation_rad = (robot_radius+safety_distance)/raw_map->resolution;  
  float dilation_rad_extra = (robot_radius+safety_distance+extra_dilation)/raw_map->resolution;  
  int dilation_rad_int = (int)dilation_rad+1;
  int dilate_r_start, dilate_r_end, dilate_c_start, dilate_c_end;
  int width = raw_map->width;
  int height = raw_map->height;
  float** raw_map_cost = raw_map->cost;
  for(int r = 0; r < height; r++)
  {
    dilate_r_start = r - dilation_rad_int;
    if(dilate_r_start < 0)
      dilate_r_start = 0;
    if(dilate_r_start > height)
      dilate_r_start = height;      
    
    dilate_r_end = r + dilation_rad_int;
    if(dilate_r_end < 0)
      dilate_r_end = 0;
    if(dilate_r_end > height)
      dilate_r_end = height;        
            
    for(int c = 0; c < width; c++)
    {
      dilate_c_start = c - dilation_rad_int;
      if(dilate_c_start < 0)
        dilate_c_start = 0;
      if(dilate_c_start > width)
        dilate_c_start = width;      
    
      dilate_c_end = c + dilation_rad_int;
      if(dilate_c_end < 0)
        dilate_c_end = 0;
      if(dilate_c_end > width)
        dilate_c_end = width;   
      
      float max_val = -1;
      
      if(!high_cost_safety_region)
      {
        for(int dr = dilate_r_start; dr < dilate_r_end; dr++)
        {
          for(int dc = dilate_c_start; dc < dilate_c_end; dc++)
          { 
            if(sqrt(((dr - r)*(dr - r)) + ((dc - c)*(dc - c))) > dilation_rad)
              continue;
            
            if(raw_map_cost[dr][dc] > max_val)
              max_val = raw_map_cost[dr][dc];
          }
        } 
      }
      else // using high_cost_safety_region
      {
        for(int dr = dilate_r_start; dr < dilate_r_end; dr++)
        {
          for(int dc = dilate_c_start; dc < dilate_c_end; dc++)
          { 
            float this_dist = sqrt(((dr - r)*(dr - r)) + ((dc - c)*(dc - c)));  
            if(this_dist > dilation_rad_extra)
              continue;
            else if(this_dist > dilation_rad)
            {
                
              if(extra_dilation_mult_from_dist)
                extra_dilation_mult = (this_dist)/(dilation_rad_extra);
                 
              if(raw_map_cost[dr][dc]*extra_dilation_mult > max_val)
                max_val = raw_map_cost[dr][dc]*extra_dilation_mult;  
            }
            else
            {
              if(raw_map_cost[dr][dc] > max_val)
                max_val = raw_map_cost[dr][dc]; 
            }  
          }
        } 
      }
      
      map[r][c] = (double)max_val;
    } 
  }
}


// this transfers the data with given bounds between raw_map and map (the search map) and dilates it
// by robot_radius + safety_distance during the transfer. 
// if 'update_search_tree' == true, this updates the search tree and propagates information change
void update_search_map_and_tree(int* y_bound, int* x_bound, bool update_search_tree)
{
  if(raw_map == NULL)
  {
    printf(" trying to update from invalid raw map\n"); 
    return;
  }
  
  if(map == NULL)
  {
    printf(" trying to update to invalid map\n"); 
    return;
  }
  //printf("propogating changes \n");
  float dilation_rad = (robot_radius+safety_distance)/raw_map->resolution;
  float dilation_rad_extra = (robot_radius+safety_distance+extra_dilation)/raw_map->resolution;  
  int dilation_rad_int = (int)dilation_rad+1;
  int height = raw_map->height;
  int width = raw_map->width;
  
  int y_start = y_bound[0] - dilation_rad_int;
  if(y_start < 0)
    y_start = 0;
  if(y_start > height)
    y_start = height;
  
  int y_end = y_bound[1] + dilation_rad_int;
  if(y_end < 0)
    y_end = 0;
  if(y_end > height)
    y_end = height;
  
  int x_start = x_bound[0] - dilation_rad_int;
  if(x_start < 0)
    x_start = 0;
  if(x_start > width)
    x_start = width;
  
  int x_end = x_bound[1] + dilation_rad_int;
  if(x_end < 0)
    x_end = 0;
  if(x_end > width)
    x_end = width;
  
  int dilate_r_start, dilate_r_end, dilate_c_start, dilate_c_end;
  float** raw_map_cost = raw_map->cost;
  for(int r = y_start; r < y_end; r++)
  {
    dilate_r_start = r - dilation_rad_int;
    if(dilate_r_start < 0)
      dilate_r_start = 0;
    if(dilate_r_start > height)
      dilate_r_start = height;      
    
    dilate_r_end = r + dilation_rad_int;
    if(dilate_r_end < 0)
      dilate_r_end = 0;
    if(dilate_r_end > height)
      dilate_r_end = height;        
            
    for(int c = x_start; c < x_end; c++)
    {
      dilate_c_start = c - dilation_rad_int;
      if(dilate_c_start < 0)
        dilate_c_start = 0;
      if(dilate_c_start > width)
        dilate_c_start = width;      
    
      dilate_c_end = c + dilation_rad_int;
      if(dilate_c_end < 0)
        dilate_c_end = 0;
      if(dilate_c_end > width)
        dilate_c_end = width;   
      
      float max_val = -1;
      if(!high_cost_safety_region)
      {
        for(int dr = dilate_r_start; dr < dilate_r_end; dr++)
        {
          for(int dc = dilate_c_start; dc < dilate_c_end; dc++)
          { 
            if(sqrt(((dr - r)*(dr - r)) + ((dc - c)*(dc - c))) > dilation_rad)
              continue;
            
            if(raw_map_cost[dr][dc] > max_val)
              max_val = raw_map_cost[dr][dc];
          }
        } 
      }
      else // using high_cost_safety_region
      {
        for(int dr = dilate_r_start; dr < dilate_r_end; dr++)
        {
          for(int dc = dilate_c_start; dc < dilate_c_end; dc++)
          { 
            if(sqrt(((dr - r)*(dr - r)) + ((dc - c)*(dc - c))) > dilation_rad_extra)
              continue;
            else if(sqrt(((dr - r)*(dr - r)) + ((dc - c)*(dc - c))) > dilation_rad)
            {
              if(raw_map_cost[dr][dc]*extra_dilation_mult > max_val)
                max_val = raw_map_cost[dr][dc]*extra_dilation_mult;  
            }
            else
            {
              if(raw_map_cost[dr][dc] > max_val)
                max_val = raw_map_cost[dr][dc]; 
            }  
          }
        } 
      }

      if(max_val != map[r][c]) // this position has changed in the dilated search map
      {
        if(update_search_tree)
           updadeCell(r, c, (double)max_val);
        
        map[r][c] = (double)max_val;
      }
    } 
  }
}

// This sets up the global data structures to perform a new search from scratch.
// It will clean up any old things that were part of a previous search. 
// This must be called after the map has changed size or the goal changes. 
// It assumes that raw_map, robot and goal pose all exist.
// Returns true when successful
// if reuse_map == true, then the map info is saved (saves on map dilation time, but only should be used for a goal change)
bool initialize_search(bool reuse_map)
{  
   printf("Initializing Search to Goal, building search tree. \n");
    
   // in case of re-initialization, clean up old values
   if(primaryCellPathHeap != NULL)
     cpDeleteHeap(primaryCellPathHeap);
   if(secondaryCellPathHeap != NULL)
     cpDeleteHeap(secondaryCellPathHeap);
   if(heapNode != NULL)
     deleteHeap(); 
   if(graph != NULL)
   {
     if(reuse_map)
       deleteGraphAndMap(false);
     else
       deleteGraphAndMap();  
   }
   else
     reuse_map = false; // if graph is null, then map is most likely not allocated

   // init map and graph
   buildGraphAndMap(raw_map->height, raw_map->width);

   // load raw_map data into the new search map, dilating it by 1/2 robot rad + safety distance
   if(!reuse_map)
     populate_map_from_raw_map();
   
   // remember start and goal locations
   double resolution = raw_map->resolution;
   int goalN = goal_pose->y/resolution; // need to add global offset ability
   if(goalN > (int)raw_map->height)
     goalN = (int)raw_map->height;
   if(goalN < 0)
     goalN = 0;
   
   int goalM = goal_pose->x/resolution; // need to add global offset ability
   if(goalM > (int)raw_map->width)
     goalM = (int)raw_map->width;
   if(goalM < 0)
     goalM = 0;
        
   int robotN = robot_pose->y/resolution; // need to add global offset ability
   if(robotN > (int)raw_map->height)
     robotN = (int)raw_map->height;
   if(robotN < 0)
     robotN = 0;
   
   int robotM = robot_pose->x/resolution; // need to add global offset ability
   if(robotM > (int)raw_map->width)
     robotM = (int)raw_map->width;
   if(robotM < 0)
     robotM = 0;
    
   // find the exact coordinates of the robot
   robot_pos_x = robot_pose->x/resolution; // need to add global offset ability
   if(robot_pos_x > raw_map->width)
     robot_pos_x = raw_map->width;
   if(robot_pos_x < 0)
     robot_pos_x = 0;
   
   robot_pos_y = robot_pose->y/resolution; // need to add global offset ability
   if(robot_pos_y > raw_map->width)
     robot_pos_y = raw_map->width;
   if(robot_pos_y < 0)
     robot_pos_y = 0;

   // init heap
   buildHeap(); 

   // init search values
   s_start = &graph[robotN][robotM];
   s_start->g = LARGE;
   s_start->rhs = LARGE;
   
   s_goal = &graph[goalN][goalM];
   s_goal->g = LARGE;
   s_goal->rhs = 0;
   insert(s_goal,calculateKey(s_goal));  

   // compute the initial field costs
   computeShortestPath();

   // create the cellPath search and look ahead heaps
   primaryCellPathHeap = cpBuildHeap();
   secondaryCellPathHeap = cpBuildHeap();
   
   return true;
}

/*--------------------------- ROS callbacks -----------------------------*/
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{    
  if(raw_map == NULL)
    return;

  if((int)(msg->pose.position.x/raw_map->resolution) < 1)
    return;
  if((int)(msg->pose.position.x/raw_map->resolution) > raw_map->width - 1)
    return;
  if((int)(msg->pose.position.y/raw_map->resolution) < 1)
    return;
  if((int)(msg->pose.position.y/raw_map->resolution) > raw_map->height - 1)
    return;

  if(robot_pose == NULL)
    robot_pose = make_pose(0,0,0);
  else
  {
    float dx = msg->pose.position.x - robot_pose->x;
    float dy = msg->pose.position.y - robot_pose->y;
    if(sqrt(dx*dx + dy*dy)/raw_map->resolution > MAX_POSE_JUMP)
    {
      // it is probably easier to replan from scratch than to update the current search tree
      // so we'll pretend we got a new goal, which will cause that to happen.
      printf("too far of a pose jump, so rebuilding tree from scratch \n");
      new_goal = true;  
    }
  }
  
  
  while(change_token_used)
    {printf(" change token used, pose \n");}
  change_token_used = true;

  robot_pose->x = msg->pose.position.x;
  robot_pose->y = msg->pose.position.y;
  robot_pose->z = msg->pose.position.z;
  robot_pose->qw = msg->pose.orientation.w;
  robot_pose->qx = msg->pose.orientation.x;
  robot_pose->qy = msg->pose.orientation.y;
  robot_pose->qz = msg->pose.orientation.z; 
  
  float qw = robot_pose->qw;
  float qx = robot_pose->qx;
  float qy = robot_pose->qy;
  float qz = robot_pose->qz; 
  
  robot_pose->cos_alpha = qw*qw + qx*qx - qy*qy - qz*qz;
  robot_pose->sin_alpha = 2*qw*qz + 2*qx*qy;
  robot_pose->alpha = atan2(robot_pose->sin_alpha, robot_pose->cos_alpha);
  //printf(" new pose: \n");
  //print_pose(robot_pose);
  //getchar();
  change_token_used = false;
}

void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{   
  if(raw_map == NULL)
    return;
 
  if((int)(msg->pose.position.x/raw_map->resolution) < 1)
    return;
  if((int)(msg->pose.position.x/raw_map->resolution) > raw_map->width - 1)
    return;
  if((int)(msg->pose.position.y/raw_map->resolution) < 1)
    return;
  if((int)(msg->pose.position.y/raw_map->resolution) > raw_map->height - 1)
    return;

  if(goal_pose == NULL)
    goal_pose = make_pose(0,0,0);
  else if(goal_pose->x == msg->pose.position.x && 
          goal_pose->y == msg->pose.position.y &&
          goal_pose->z == msg->pose.position.z && 
          goal_pose->qw == msg->pose.orientation.w && 
          goal_pose->qx == msg->pose.orientation.x && 
          goal_pose->qy == msg->pose.orientation.y && 
          goal_pose->qz == msg->pose.orientation.z) // if it is the same as the old pose, then do nothing
      return;
  
  printf("new goal -------------------------------------------------------------------\n");
    
  
  
  while(change_token_used)
    {printf(" change token used, goal \n");}
  change_token_used = true;

  goal_pose->x = msg->pose.position.x;
  goal_pose->y = msg->pose.position.y;
  goal_pose->z = msg->pose.position.z;
  goal_pose->qw = msg->pose.orientation.w;
  goal_pose->qx = msg->pose.orientation.x;
  goal_pose->qy = msg->pose.orientation.y;
  goal_pose->qz = msg->pose.orientation.z; 
  
  float qw = goal_pose->qw;
  float qx = goal_pose->qx;
  float qy = goal_pose->qy;
  float qz = goal_pose->qz; 
  
  goal_pose->cos_alpha = qw*qw + qx*qx - qy*qy - qz*qz;
  goal_pose->sin_alpha = 2*qw*qz + 2*qx*qy;
  goal_pose->alpha = atan2(goal_pose->sin_alpha, goal_pose->cos_alpha);
  
  //print_pose(goal_pose);
  new_goal = true;

  change_token_used = false;
}

void map_changes_callback(const sensor_msgs::PointCloud::ConstPtr& msg)
{   
  if(raw_map == NULL || robot_pose == NULL || goal_pose == NULL)
    return;
        
  int length = msg->points.size();

  while(change_token_used)
    {printf("change token used, map changes \n");}
  change_token_used = true;

  float** cost = raw_map->cost;
  int width = raw_map->width;
  int height = raw_map->height;
  int y, x;
  
  // remember what part of the map has been changed
  int y_min = raw_map->height;
  int y_max = 0;
  int x_min = raw_map->width;
  int x_max = 0;
  
  for(int i = 0; i < length; i++)
  {
    y = (int)msg->points[i].y;
    x = (int)msg->points[i].x;
            
    if(x >= 0 && x < width && y >= 0 && y < height)
    {
      cost[y][x] = prob_to_cost(msg->points[i].z);
      
      if(y < y_min)
        y_min = y;
      if(y > y_max)
        y_max = y;
      if(x < x_min)
        x_min = x;
      if(x > x_max)
        x_max = x;
    }
  }

  // find how these translate to dilated changes in map (the search map) and then update relevant cells and the search tree
  int y_bound[] = {y_min, y_max+1};
  int x_bound[] = {x_min, x_max+1};
  update_search_map_and_tree(y_bound, x_bound, true);
  
  change_token_used = false;
}


/*---------------------- ROS Publisher Functions ------------------------*/
void publish_global_path(MapPath* path)
{
  while(change_token_used)
    {printf("change token used, publish path \n");}
  change_token_used = true;

  int length = path->length;
    
  nav_msgs::Path msg;  
  
  msg.header.frame_id = "/map_cu";
  msg.poses.resize(length);

  float resolution = raw_map->resolution;
  for(int i = 0; i < length; i++)
  {
  
    msg.poses[i].pose.position.x = path->x[i]*resolution; // need to add global offset ability
    msg.poses[i].pose.position.y = path->y[i]*resolution; // need to add global offset ability
    msg.poses[i].pose.position.z = 0;
  
  } 
  global_path_pub.publish(msg); 

  //printf("sending global path %d \n",length);
  change_token_used = false;
}

void publish_system_update(int data)
{
  std_msgs::Int32 msg;  
  
  msg.data = data;
  system_update_pub.publish(msg); 
}


/*----------------------- ROS service functions -------------------------*/
// requests the map from the mapping system, stores it in raw_map
bool load_map()
{
  nav_msgs::GetMap::Request  req;
  nav_msgs::GetMap::Response resp;
  ROS_INFO("Requesting the map...\n");
  if( !ros::service::call("/cu/get_map_cu", req, resp) )
  {
    ROS_INFO("request failed\n");
    return NULL;
  }
  
  ROS_INFO("Received a %d X %d map @ %.3f m/pix\n", resp.map.info.width, resp.map.info.height, resp.map.info.resolution);

  if (resp.map.info.width == 0 && resp.map.info.height == 0)
    return false;
  
  if(raw_map != NULL) // the map has been reloaded, so we need to clean up memory of old raw_map
    destroy_map(raw_map);
  
  while(change_token_used)
    {printf("change token used, load map \n");}
  change_token_used = true;

  float map_resolution = resp.map.info.resolution;
  int map_width = resp.map.info.width;
  int map_height = resp.map.info.height;

  raw_map = make_map(map_height, map_width, map_resolution);
     
  float** raw_map_cost = raw_map->cost;
  for(int i = 0; i < map_height; i++)
    for(int j = 0; j < map_width; j++)
      raw_map_cost[i][j] = prob_to_cost(((float)resp.map.data[i*map_width+j])/100);

  change_token_used = false;
  
  return true;
}

bool load_pose()
{
  localization_cu::GetPose::Request req;
  localization_cu::GetPose::Response resp;
  //ROS_INFO("Requesting the pose...\n");
  if( !ros::service::call("/cu/get_pose_cu", req, resp) )
  {
    ROS_INFO("load pose failed\n");
    return false;
  }
  
   if(raw_map == NULL)
    return false;

  if((int)(resp.pose.pose.position.x/raw_map->resolution) < 1)
    return false;
  if((int)(resp.pose.pose.position.x/raw_map->resolution) > raw_map->width - 1)
    return false;
  if((int)(resp.pose.pose.position.y/raw_map->resolution) < 1)
    return false;
  if((int)(resp.pose.pose.position.y/raw_map->resolution) > raw_map->height - 1)
    return false;

  if(robot_pose == NULL)
    robot_pose = make_pose(0,0,0);
  else
  {
    float dx = resp.pose.pose.position.x - robot_pose->x;
    float dy = resp.pose.pose.position.y - robot_pose->y;
    if(sqrt(dx*dx + dy*dy)/raw_map->resolution > MAX_POSE_JUMP)
    {
      // it is probably easier to replan from scratch than to update the current search tree
      // so we'll pretend we got a new goal, which will cause that to happen.
      printf("too far of a pose jump, so rebuilding tree from scratch (service) \n");
      new_goal = true;  
    }
  }
  
  robot_pose->x = resp.pose.pose.position.x;
  robot_pose->y = resp.pose.pose.position.y;
  robot_pose->z = resp.pose.pose.position.z;
  robot_pose->qw = resp.pose.pose.orientation.w;
  robot_pose->qx = resp.pose.pose.orientation.x;
  robot_pose->qy = resp.pose.pose.orientation.y;
  robot_pose->qz = resp.pose.pose.orientation.z; 
  
  float qw = robot_pose->qw;
  float qx = robot_pose->qx;
  float qy = robot_pose->qy;
  float qz = robot_pose->qz; 
  
  robot_pose->cos_alpha = qw*qw + qx*qx - qy*qy - qz*qz;
  robot_pose->sin_alpha = 2*qw*qz + 2*qx*qy;
  robot_pose->alpha = atan2(robot_pose->sin_alpha, robot_pose->cos_alpha);
  //printf(" new pose: \n");
  //print_pose(robot_pose);
  //getchar();
  
  return true;
}

bool load_goal()
{
  localization_cu::GetPose::Request req;
  localization_cu::GetPose::Response resp;
  //ROS_INFO("Requesting the pose...\n");
  if( !ros::service::call("/cu/get_goal_cu", req, resp) )
  {
    ROS_INFO("load goal failed\n");
    return false;
  }
  
   if(raw_map == NULL)
    return false;
   else if(goal_pose->x == resp.pose.pose.position.x && 
          goal_pose->y == resp.pose.pose.position.y &&
          goal_pose->z == resp.pose.pose.position.z && 
          goal_pose->qw == resp.pose.pose.orientation.w && 
          goal_pose->qx == resp.pose.pose.orientation.x && 
          goal_pose->qy == resp.pose.pose.orientation.y && 
          goal_pose->qz == resp.pose.pose.orientation.z) // if it is the same as the old pose, then do nothing
   return false;
  
  printf("new goal via service -------------------------------------------------------------------\n");
  
  if((int)(resp.pose.pose.position.x/raw_map->resolution) < 1)
    return false;
  if((int)(resp.pose.pose.position.x/raw_map->resolution) > raw_map->width - 1)
    return false;
  if((int)(resp.pose.pose.position.y/raw_map->resolution) < 1)
    return false;
  if((int)(resp.pose.pose.position.y/raw_map->resolution) > raw_map->height - 1)
    return false;

  if(robot_pose == NULL)
    robot_pose = make_pose(0,0,0);

  goal_pose->x = resp.pose.pose.position.x;
  goal_pose->y = resp.pose.pose.position.y;
  goal_pose->z = resp.pose.pose.position.z;
  goal_pose->qw = resp.pose.pose.orientation.w;
  goal_pose->qx = resp.pose.pose.orientation.x;
  goal_pose->qy = resp.pose.pose.orientation.y;
  goal_pose->qz = resp.pose.pose.orientation.z; 
  
  float qw = goal_pose->qw;
  float qx = goal_pose->qx;
  float qy = goal_pose->qy;
  float qz = goal_pose->qz; 
  
  goal_pose->cos_alpha = qw*qw + qx*qx - qy*qy - qz*qz;
  goal_pose->sin_alpha = 2*qw*qz + 2*qx*qy;
  goal_pose->alpha = atan2(goal_pose->sin_alpha, goal_pose->cos_alpha);
  //printf(" new goal: \n");
  //print_pose(goal_pose);
  //getchar();
  
  new_goal = true;
    
  return true;
}

int main(int argc, char** argv) 
{
  ros::init(argc, argv, "base_planner_cu");
  ros::NodeHandle nh;
  ros::Rate loop_rate(100);
    
  // load globals from parameter server
  double param_input;
  bool bool_input;
  if(ros::param::get("base_planner_cu/obstacle_cost", param_input))
    OBSTACLE_COST = param_input;                                                   // the cost of an obstacle
  if(ros::param::get("base_planner_cu/robot_radius", param_input)) 
    robot_radius = param_input;                                                    //(m)
  if(ros::param::get("base_planner_cu/safety_distance", param_input)) 
    safety_distance = param_input;                                                 //(m), distance that must be maintained between robot and obstacles
  if(ros::param::get("base_planner_cu/better_path_ratio", param_input)) 
    old_path_discount = param_input;                                               // if (current path length)*old_path_discount < (old path length) < (current path length)/old_path_discount, then stick with the old path
  if(ros::param::get("base_planner_cu/replan_jump", param_input)) 
    MAX_POSE_JUMP = param_input;                                                   //(map grids) after which it is easer to replan from scratch
  if(ros::param::get("base_planner_cu/using_extra_safety_distance", bool_input)) 
    high_cost_safety_region = bool_input;                                          // true: dilate obstacles by an extra extra_dilation but instad of lethal, multiply existing cost by extra_dilation_mult
  if(ros::param::get("base_planner_cu/extra_safety_distance", param_input)) 
    extra_dilation = param_input;                                                  //(m)

  // print data about parameters
  printf("obstacle cost: %f, robot radius: %f, safety_distance: %f, extra_safety_distance: %f, \nbetter_path_ratio: %f, replan_jump: %f \n", OBSTACLE_COST, robot_radius, safety_distance, extra_dilation, old_path_discount, MAX_POSE_JUMP);
  if(high_cost_safety_region)
    printf("using extra safety distance \n");
  else
    printf("not using extra safety distance \n");
  
  // wait until the map service is provided (we need its tf /world_cu -> /map_cu to be broadcast)
  ros::service::waitForService("/cu/get_map_cu", -1);
  
  // set up ROS topic subscriber callbacks
  pose_sub = nh.subscribe("/cu/pose_cu", 1, pose_callback);
  goal_sub = nh.subscribe("/cu/goal_cu", 1, goal_callback);
  map_changes_sub = nh.subscribe("/cu/map_changes_cu", 10, map_changes_callback);
    
  // set up ROS topic publishers
  global_path_pub = nh.advertise<nav_msgs::Path>("/cu/global_path_cu", 1);
  system_update_pub = nh.advertise<std_msgs::Int32>("/cu/system_update_cu", 10);
  
  // spin ros once
  ros::spinOnce();
  loop_rate.sleep(); 
  
  // wait for a map
  while(!load_map() && ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep(); 
  }
  
  // init map
  buildGraphAndMap(raw_map->height, raw_map->width);
  populate_map_from_raw_map();
  
  // wait until we have a goal and a robot pose (these arrive via callbacks)
  while((goal_pose == NULL || robot_pose == NULL) && ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();    
  }
    
  // initialize search and related data structures (this calculates initial cost field)
  if(ros::ok())
  {
    initialize_search(true);  // true := use old map, (raw_map was populated in a pre-processing step)
    new_goal = false; 
    ros::spinOnce();  // check in on callbacks
    
    // added to give map changes a chance to propogate before first real search
    ros::Rate one_time_sleep(2);
    one_time_sleep.sleep();
    
    ros::spinOnce();  // check in on callbacks
  }
  
  // main planning loop
  int lr, ud;
  int time_counter = 0;
  MapPath* old_path = NULL;
  while (ros::ok()) 
  {
    while(change_token_used)
      {printf("change token used, main \n");}
    change_token_used = true;

    time_counter++;
    //printf(" This is the base planner %d\n", time_counter); // heartbeat for debugging

    load_goal();
    
    if(new_goal || reload_map)
    {  
      if(reload_map)
      {
        printf("reinitializing map and searchtree \n");
        
        change_token_used = false;
        // wait for a map
        while(!load_map() && ros::ok())
        {
          ros::spinOnce();
          loop_rate.sleep(); 
        }
        change_token_used = true;
        
        initialize_search(false); // false := reset map based in what is in raw_map (raw_map was just re-populated with info from the map server)
      }
      else // new_goal
      {
        printf("reinitializing search tree, reusing map \n");
        initialize_search(true); // true := reuse the old map
      }
      
      new_goal = false;   
      reload_map = false;
 
      if(old_path != NULL)
      {
        deleteMapPath(old_path);
        old_path = NULL;
      }
      
      // get best estimate of pose
      while(!load_pose())
      {
        // wait for most up to date pose
      }
      printf(" recieved pose via service \n");       
    }
    else
      load_pose(); // if services lag then this is a bad idea, but have had problems on netbooks never getting new pose
    
    // find the new exact coordinates of the robot 
    robot_pos_x = robot_pose->x/raw_map->resolution; // need to add global offset ability
    if(robot_pos_x > raw_map->width)
      robot_pos_x = raw_map->width;
    if(robot_pos_x < 0)
      robot_pos_x = 0;
   
    robot_pos_y = robot_pose->y/raw_map->resolution; // need to add global offset ability
    if(robot_pos_y > raw_map->width)
      robot_pos_y = raw_map->width;
    if(robot_pos_y < 0)
      robot_pos_y = 0;  
 
        
    // do replanning, note that we need to make sure that all neighboring nodes of the cell(s) containing the robot are updated 
    //printf("replanning %d \n", time_counter);
    if(robot_pos_x == (double)((int)robot_pos_x)) // then on a vertical edge, need to plan to nodes on cells to the left and right of the robot
      lr = -1;
    else //only need to plan to nodes on cells with x == int_pos_x
      lr = 0;
    
    while(lr < 2)
    { 
      if(robot_pos_y == (double)((int)robot_pos_y)) // then on a horizontal edge, need to plan to nodes on cells to the top and bottom of the robot
        ud = -1;
      else //only need to plan to nodes on cells with y == int_pos_y
        ud = 0;

      if((int)robot_pos_x + lr < 0 || (int)robot_pos_x + lr > WIDTH)
      {
        lr++;
        continue;
      }

      while(ud < 2)
      { 
        if((int)robot_pos_y + ud < 0 || (int)robot_pos_y + ud > HEIGHT)
        {
          ud++;
          continue;
        }

        s_start = &graph[(int)robot_pos_y + ud][(int)robot_pos_x + lr];
         
        computeShortestPath();  // this updates the cost field to this node
        ud++;  
      }
      lr++;
    }  
       
    // extract the path, this will be used to figure out where to move the robot  
    //printf("extract path1 %d \n", time_counter);
    MapPath* pathToGoalWithLookAhead = extractPathOneLookahead();
    double path1_max_single_grid;
    double path1_cost = calculatePathCost(pathToGoalWithLookAhead, path1_max_single_grid);

    //printf("extract path2 %d \n", time_counter);
    MapPath* pathToGoalMine = extractPathMine(0); // this uses gradient descent where possible
    double path2_max_single_grid;
    double path2_cost = calculatePathCost(pathToGoalMine, path2_max_single_grid);

    
    double old_path_cost = LARGE;
    double old_path_max_single_grid;
    if(old_path != NULL)
      old_path_cost = calculatePathCost(old_path, old_path_max_single_grid);
    
    change_token_used = false;

    // use better path of the two to move the robot (or retain the old path if it is still better)
    if(old_path != NULL && path1_cost*old_path_discount <= old_path_cost && old_path_cost <= path1_cost/old_path_discount
                        && path2_cost*old_path_discount <= old_path_cost && old_path_cost <= path2_cost/old_path_discount 
                        && old_path_max_single_grid < OBSTACLE_COST)
    {
      publish_global_path(old_path);
      //printf("old path is about the same or better %d [%f vs %f] %f \n", time_counter, old_path_cost, (path1_cost+path2_cost)/2, old_path_max_single_grid);   
        
      deleteMapPath(pathToGoalMine);  
      deleteMapPath(pathToGoalWithLookAhead);
    }
    else if(path1_cost < path2_cost && path1_max_single_grid < OBSTACLE_COST)
    {         
      publish_global_path(pathToGoalWithLookAhead); 
      //printf("path1 is better %d %f %f\n", time_counter, path1_cost, path1_max_single_grid);

      
      if(old_path != NULL)
      {
        deleteMapPath(old_path);
        old_path = NULL;
      }
      
      old_path = pathToGoalWithLookAhead;
      deleteMapPath(pathToGoalMine);
    }
    else if(path2_max_single_grid < OBSTACLE_COST)
    {
      publish_global_path(pathToGoalMine);  
      //printf("path2 is better %d %f %f\n", time_counter, path2_cost, path2_max_single_grid);
      
      if(old_path != NULL)
      {
        deleteMapPath(old_path);
        old_path = NULL;
      }
      
      old_path = pathToGoalMine;
      deleteMapPath(pathToGoalWithLookAhead);
    }
    else // all paths go through obstacles
    {
     printf("Base Planner: No safe path exists to goal %f %f %f\n", old_path_cost, path1_cost, path2_cost);   
     publish_system_update(1);   
     reload_map = true;
    }

    ros::spinOnce();
    loop_rate.sleep();
    
    //printf("done %d \n", time_counter);
  }
    
  if(old_path != NULL)
    deleteMapPath(old_path);
  
  pose_sub.shutdown();
  goal_sub.shutdown();
  map_changes_sub.shutdown();
  global_path_pub.shutdown();
  system_update_pub.shutdown();
    
  destroy_pose(robot_pose);
  destroy_pose(goal_pose);
    
  cpDeleteHeap(primaryCellPathHeap);
  cpDeleteHeap(secondaryCellPathHeap);
  deleteHeap();   
  deleteGraphAndMap();
}
